#include <ros/ros.h>
#include <mavros_msgs/PositionTarget.h>
#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/NavSatFix.h>
#include <tf/tf.h>
#include <math.h>

// WGS84常量
const double R_EARTH = 6371000; // 地球半径，单位为米

// 将经纬度转换为ENU（东-北-上）坐标
void globalToLocalENU(double lat1, double lon1, double lat2, double lon2, double& dx, double& dy) {
    double dLat = (lat2 - lat1) * M_PI / 180.0;
    double dLon = (lon2 - lon1) * M_PI / 180.0;

    double latAvg = (lat1 + lat2) / 2.0 * M_PI / 180.0;
    double cosLat = cos(latAvg);

    dx = R_EARTH * dLon * cosLat;
    dy = R_EARTH * dLat;
}

// 计算无人机和目标点之间的方位角（Yaw角）
double calculateYawToTarget(double dx, double dy) {
    return atan2(dy, dx);  // 方位角，单位为弧度
}

// 进行机体坐标系旋转转换
void rotateToBodyFrame(double dx, double dy, double yaw, double& body_x, double& body_y) {
    body_x = dx * cos(yaw) + dy * sin(yaw); // 将ENU坐标系旋转到机体坐标系
    body_y = -dx * sin(yaw) + dy * cos(yaw);
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "drone_navigation_node");
    ros::NodeHandle nh;

    // 目标经纬度
    double target_lat = 47.397842;
    double target_lon = 9.5455932;
    double target_alt = 900.0;

    // 当前无人机的经纬度（假设无人机初始经纬度为当前位置）
    double current_lat = 47.397742;
    double current_lon = 9.5455932;
    double current_alt = 500.0;

    // 计算无人机从当前位置到目标点的ENU距离
    double dx, dy;
    globalToLocalENU(current_lat, current_lon, target_lat, target_lon, dx, dy);

    // 计算目标点相对于无人机的方位角
    double yaw_to_target = calculateYawToTarget(dx, dy);

    // 无人机当前的yaw角（假设此数据从飞控或姿态数据中读取）
    double current_yaw = 0.0; // 假设为0，朝向正北

    // 将目标点从ENU坐标系转换到机体坐标系
    double body_x, body_y;
    rotateToBodyFrame(dx, dy, current_yaw, body_x, body_y);

    // 发布目标位置给无人机
    ros::Publisher pos_pub = nh.advertise<mavros_msgs::PositionTarget>("mavros/setpoint_raw/local", 10);
    ros::Rate rate(20.0);

    while (ros::ok()) {
        mavros_msgs::PositionTarget pos_msg;
        pos_msg.coordinate_frame = mavros_msgs::PositionTarget::FRAME_BODY_NED;  // 使用机体坐标系
        pos_msg.type_mask = mavros_msgs::PositionTarget::IGNORE_VX |
                            mavros_msgs::PositionTarget::IGNORE_VY |
                            mavros_msgs::PositionTarget::IGNORE_VZ |
                            mavros_msgs::PositionTarget::IGNORE_AFX |
                            mavros_msgs::PositionTarget::IGNORE_AFY |
                            mavros_msgs::PositionTarget::IGNORE_AFZ |
                            mavros_msgs::PositionTarget::IGNORE_YAW |
                            mavros_msgs::PositionTarget::IGNORE_YAW_RATE;

        // 设置目标位置（相对于机体坐标系）
        pos_msg.position.x = body_x;  // 前方向移动的距离
        pos_msg.position.y = body_y;  // 侧方向移动的距离
        pos_msg.position.z = target_alt - current_alt; // 高度差

        pos_pub.publish(pos_msg);
        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}
